namespace Microsoft.Robotics.Navigation.UnitTests
{
    using System;
    using System.Collections.Generic;
    using System.IO;
    using System.Xml;
    using System.Xml.XPath;
    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.VisualStudio.TestTools.UnitTesting;
    using Test.Robotics.Metrics;
    using Test.Robotics.Navigation;
    using Test.Robotics.Runtime;

    /*
     * To add a new recording - follow below steps
     *  1. Create a new short recording with robot starting anywhere, but always stopping under a landmark (or take a note of the end pose on the loaded map from visualizer)
     *      a. Use a robot with calibrated upfacing camera
     *      b. Dont wait for more than 5 sec for gyro calibration. Short recording tests are using a smaller sample set to calibrate a gyro (100 samples currently)
     *      c. When you arrive under a landmark, wait for a second and press left bumper and quickly stop the recording
     *      d. Do not make recordings longer than 10-20 seconds, it will really defeat the purpose of quick testing. 
     *  2. Drop all recording files under \\robofile\baker\Shared\Users\roboim\Recordings\Shorts\Recordings
     *      a. Rename the recordings files as usual to be consumable by playback agents
     *      b. Recommended: Add a short readme txt file describing in plain english as to what happened during the recording to the same folder where the recordings are
     *  3. Add a new entry for the recording in the database
     *      a. Remote onto B115BKRSQL01
     *      b. Open MS SQL mgmt studio, and open Databases->Tables->Metrics->Dbo.Configuration
     *      c. Copy an existing, or create a new entry in the DB for your recording following the pattern you see for other entries that have have a Type ShortRecordingNavTest
     *          c1. Take a look at all columns, they are self-explanatory. Usually, unless you change a map or robot - you only have to change one entry - the name of the folder where recordings are
     *  4. Add a new set of UTs here based on nre recording
     *      a. Get a robot start pose from the map (either using Paint or a similar tool and inspecting the bitmap, or using our UI (that work is still TBD)
     *          * Note: Robot start pose is in meters and radians relative to map origin. Ground truth message uses the same units and the same origin
     *      b. Add as many test variations as you wish for this recording. I.e. see how we do it for other recordings (i.e. vary start pose variance)
     *  5. IMPORTANT: To simplify debugging and troubleshooting - if a test fails, a repro manifest will be generated under C:\Temp\FailedShortRunManifests
     *      with the name of the recording and test variation that failed - just kick off that manifest, and see the repro dont forget the /o parameter
     */

    /// <summary>
    /// Short recording navigation tests.
    /// Note, this suit takes a while to run. Each individual test takes about a minute.
    /// </summary>
    [TestClass]
    [DeploymentItem(@"Microsoft.Robotics.Vision.Native.dll")]
    [DeploymentItem(@"Microsoft.Robotics.Vision.Interop.dll")]
    [DeploymentItem(@"Data\GroundTruthExtraction\calib_board.txt", @"Data\GroundTruthExtraction")]
    public class ShortNavigationTests
    {
        /// <summary>
        /// Runs a recording from DB config with specified start pose settings.
        /// If a test fails - we generate a 'repro' manifest with settings that made it fail
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Metrics")]
        [TestCategory("Slow")]
        [TestCategory("ShortRecording")]
        public void HallwayUpToConfRoom2167_SmallStartPoseVariance()
        {
            Assert.IsTrue(
                this.ShortRecordingLocalizationIsWithinThreshold(
                    recordingFolderName: "HallwayUpToConfRoom2167",
                    testVariationDescription: "SmallVariance",
                    targetLandmarkID: 0,
                    startPoseInMetersAndRads: new Pose2D(18, 46.6, 0),
                    startPoseVarianceInMetersAndRads: new Pose2D(0.2, 0.2, 0.1),
                    particleCount: 200,
                    translationErrorThreshold: 0.3,
                    headingErrorThreshold: 0.2,
                    manuallySetFinishPose: new Pose2D(19.1, 53.5, Math.PI / 2)));
        }

        /// <summary>
        /// Runs a recording from DB config with specified start pose settings.
        /// If a test fails - we generate a 'repro' manifest with settings that made it fail
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Metrics")]
        [TestCategory("Slow")]
        [TestCategory("ShortRecording")]
        public void CenterOfHallwayToLM0WithManualPush_SmallStartPoseVariance()
        {
            // This test should fail, since presently, we can not disambiguate between manual push towards an obstacle
            // and obstacle moving towards the robot (i.e. people)
            Assert.IsFalse(
                this.ShortRecordingLocalizationIsWithinThreshold(
                    recordingFolderName: "CenterOfHallwayToLM0WithManualPush",
                    testVariationDescription: "SmallVariance",
                    targetLandmarkID: 0,
                    startPoseInMetersAndRads: new Pose2D(16, 60, Math.PI),
                    startPoseVarianceInMetersAndRads: new Pose2D(0.2, 0.2, 0.1),
                    particleCount: 200,
                    translationErrorThreshold: 1,
                    headingErrorThreshold: 0.3));
        }

        /// <summary>
        /// Runs a recording from DB config with specified start pose settings.
        /// If a test fails - we generate a 'repro' manifest with settings that made it fail
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Metrics")]
        [TestCategory("Slow")]
        [TestCategory("ShortRecording")]
        public void CornerOfAthenaToLM0CrowdedAndSlow_LargeStartPoseVariance()
        {
            Assert.IsTrue(
                this.ShortRecordingLocalizationIsWithinThreshold(
                    recordingFolderName: "CornerOfAthenaToLM0CrowdedAndSlow",
                    testVariationDescription: "LargeVariance",
                    targetLandmarkID: 0,
                    startPoseInMetersAndRads: new Pose2D(8.3, 51, 0),
                    startPoseVarianceInMetersAndRads: new Pose2D(0.5, 0.5, 0.2),
                    particleCount: 200,
                    translationErrorThreshold: 0.3,
                    headingErrorThreshold: 0.2));
        }

        /// <summary>
        /// Runs a recording from DB config with specified start pose settings.
        /// If a test fails - we generate a 'repro' manifest with settings that made it fail
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Metrics")]
        [TestCategory("Slow")]
        [TestCategory("ShortRecording")]
        public void CornerOfAthenaToLM0CrowdedAndSlow_WrongPose()
        {
            Assert.IsTrue(
                this.ShortRecordingLocalizationIsWithinThreshold(
                    recordingFolderName: "CornerOfAthenaToLM0CrowdedAndSlow",
                    testVariationDescription: "WrongPose",
                    targetLandmarkID: 0,
                    startPoseInMetersAndRads: new Pose2D(8.3 + 0.5, 51 + 0.5, 0 + 0.25), // note error introduced to start pose
                    startPoseVarianceInMetersAndRads: new Pose2D(0.0, 0.0, 0.0),
                    particleCount: 200,
                    translationErrorThreshold: 0.3,
                    headingErrorThreshold: 0.2));
        }

        /// <summary>
        /// Runs a recording from DB config with specified start pose settings.
        /// If a test fails - we generate a 'repro' manifest with settings that made it fail
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Metrics")]
        [TestCategory("Slow")]
        [TestCategory("ShortRecording")]
        public void CornerOfStansToLM0WithRotation_LargeStartPoseVariance()
        {
            Assert.IsTrue(
                this.ShortRecordingLocalizationIsWithinThreshold(
                    recordingFolderName: "CornerOfStansToLM0WithRotation",
                    testVariationDescription: "LargeVariance",
                    targetLandmarkID: 0,
                    startPoseInMetersAndRads: new Pose2D(9, 55.5, 0),
                    startPoseVarianceInMetersAndRads: new Pose2D(0.5, 0.5, 0.3),
                    particleCount: 200,
                    translationErrorThreshold: 0.3,
                    headingErrorThreshold: 0.2));
        }

        /// <summary>
        /// Runs a recording from DB config with specified start pose settings.
        /// If a test fails - we generate a 'repro' manifest with settings that made it fail
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Metrics")]
        [TestCategory("Slow")]
        [TestCategory("ShortRecording")]
        public void CornerOfStansToLM0WithRotation_WrongPose()
        {
            Assert.IsTrue(
                this.ShortRecordingLocalizationIsWithinThreshold(
                    recordingFolderName: "CornerOfStansToLM0WithRotation",
                    testVariationDescription: "WrongPose",
                    targetLandmarkID: 0,
                    startPoseInMetersAndRads: new Pose2D(9 + 0.5, 55.5 + 0.5, 0 + 0.25), // note error introduced to start pose
                    startPoseVarianceInMetersAndRads: new Pose2D(0.0, 0.0, 0.0),
                    particleCount: 200,
                    translationErrorThreshold: 0.3,
                    headingErrorThreshold: 0.2));
        }

        /// <summary>
        /// Runs a recording from DB config with specified start pose settings.
        /// If a test fails - we generate a 'repro' manifest with settings that made it fail
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Metrics")]
        [TestCategory("Slow")]
        [TestCategory("ShortRecording")]
        public void CornerOfStansOfficeToLM0_LargeStartPoseVariance()
        {
            Assert.IsTrue( 
                this.ShortRecordingLocalizationIsWithinThreshold(
                    recordingFolderName: "CornerOfStansOfficeToLM0",
                    testVariationDescription: "LargeVariance",
                    targetLandmarkID: 0,
                    startPoseInMetersAndRads: new Pose2D(8.7, 55, Math.PI / 2),
                    startPoseVarianceInMetersAndRads: new Pose2D(0.5, 0.5, 0.3),
                    particleCount: 200,
                    translationErrorThreshold: 0.3,
                    headingErrorThreshold: 0.2));
        }

        /// <summary>
        /// Runs a recording from DB config with specified start pose settings.
        /// If a test fails - we generate a 'repro' manifest with settings that made it fail
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Metrics")]
        [TestCategory("Slow")]
        [TestCategory("ShortRecording")]
        public void CornerOfStansOfficeToLM0_WrongPose()
        {
            Assert.IsTrue(
                this.ShortRecordingLocalizationIsWithinThreshold(
                    recordingFolderName: "CornerOfStansOfficeToLM0",
                    testVariationDescription: "WrongPose",
                    targetLandmarkID: 0,
                    startPoseInMetersAndRads: new Pose2D(8.7 + 0.5, 55 + 0.5, Math.PI / 2 + 0.25), // note error introduced to start pose
                    startPoseVarianceInMetersAndRads: new Pose2D(0.0, 0.0, 0.0),
                    particleCount: 200,
                    translationErrorThreshold: 0.5,
                    headingErrorThreshold: 0.3));
        }

        /// <summary>
        /// Runs a recording from DB config with specified start pose settings.
        /// If a test fails - we generate a 'repro' manifest with settings that made it fail
        /// In this test, 033 was originally facing south (on the map), at the center of the hallway then as it was started - 
        /// a person walked in front of it going in the same direction as the robot. Driving was mostly straight.
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Metrics")]
        [TestCategory("Slow")]
        [TestCategory("ShortRecording")]
        public void CenterOfHalwayToLM0WithPerson_LargeStartPoseVariance()
        {
            Assert.IsTrue(
                this.ShortRecordingLocalizationIsWithinThreshold(
                    recordingFolderName: "CenterOfHalwayToLM0WithPerson",
                    testVariationDescription: "LargeVariance",
                    targetLandmarkID: 0,
                    startPoseInMetersAndRads: new Pose2D(16, 60, Math.PI),
                    startPoseVarianceInMetersAndRads: new Pose2D(0.5, 0.5, 0.3),
                    particleCount: 200,
                    translationErrorThreshold: 0.3,
                    headingErrorThreshold: 0.2));
        }      

        /// <summary>
        /// Runs a recording from DB config with specified start pose settings.
        /// If a test fails - we generate a 'repro' manifest with settings that made it fail
        /// In this test, 033 was originally facing south (on the map), at the center of the hallway then as it was started - 
        /// a person walked in front of it going in the same direction as the robot. Driving was mostly straight.
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Metrics")]
        [TestCategory("Slow")]
        [TestCategory("ShortRecording")]
        public void CenterOfHalwayToLM0WithPerson_WrongPose()
        {
            Assert.IsTrue(
                this.ShortRecordingLocalizationIsWithinThreshold(
                    recordingFolderName: "CenterOfHalwayToLM0WithPerson",
                    testVariationDescription: "WrongPose",
                    targetLandmarkID: 0,
                    startPoseInMetersAndRads: new Pose2D(16 + 0.5, 60 + 0.5, Math.PI + 0.25), // note error introduced to start pose
                    startPoseVarianceInMetersAndRads: new Pose2D(0.0, 0.0, 0.0),
                    particleCount: 200,
                    translationErrorThreshold: 0.3,
                    headingErrorThreshold: 0.2));
        }

        /// <summary>
        /// Runs a recording from DB config with specified start pose settings.
        /// If a test fails - we generate a 'repro' manifest with settings that made it fail
        /// In this test, 033 was originally facing north (on the map), at the center of the hallway.
        /// It turned around and drove to the LM0 while getting fairly close to a wall on two occasions
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Metrics")]
        [TestCategory("Slow")]
        [TestCategory("ShortRecording")]
        public void CenterOfHalwayToLM0WithVeering_LargeStartPoseVariance()
        {
            Assert.IsTrue(
                this.ShortRecordingLocalizationIsWithinThreshold(
                    recordingFolderName: "CenterOfHalwayToLM0WithVeering",
                    testVariationDescription: "LargeVariance",
                    targetLandmarkID: 0,
                    startPoseInMetersAndRads: new Pose2D(16, 60, 0),
                    startPoseVarianceInMetersAndRads: new Pose2D(0.5, 0.5, 0.3),
                    particleCount: 200,
                    translationErrorThreshold: 0.3,
                    headingErrorThreshold: 0.2));
        }

        /// <summary>
        /// Runs a recording from DB config with specified start pose settings.
        /// If a test fails - we generate a 'repro' manifest with settings that made it fail
        /// In this test, 033 was originally facing north (on the map), at the center of the hallway.
        /// It turned around and drove to the LM0 while getting fairly close to a wall on two occasions
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Metrics")]
        [TestCategory("Slow")]
        [TestCategory("ShortRecording")]
        public void CenterOfHalwayToLM0WithVeering_WrongPose()
        {
            Assert.IsTrue(
                this.ShortRecordingLocalizationIsWithinThreshold(
                    recordingFolderName: "CenterOfHalwayToLM0WithVeering",
                    testVariationDescription: "WrongPose",
                    targetLandmarkID: 0,
                    startPoseInMetersAndRads: new Pose2D(16 + 0.5, 60 + 0.5, 0 + 0.25), // note error introduced to start pose
                    startPoseVarianceInMetersAndRads: new Pose2D(0.0, 0.0, 0.0),
                    particleCount: 200,
                    translationErrorThreshold: 0.3,
                    headingErrorThreshold: 0.2));
        }

        /// <summary>
        /// Runs a recording from DB config with specified start pose settings.
        /// If a test fails - we generate a 'repro' manifest with settings that made it fail
        /// In this test, 033 was originally facing north (on the map), at the center of the hallway.
        /// Robot started facing north, then turned 270 degrees CW, then 90 degrees CCW, then drove more or less straight to LM0
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Metrics")]
        [TestCategory("Slow")]
        [TestCategory("ShortRecording")]
        public void CenterOfHallwayToLM0WithRotation_LargeStartPoseVariance()
        {
            Assert.IsTrue(
                this.ShortRecordingLocalizationIsWithinThreshold(
                    recordingFolderName: "CenterOfHallwayToLM0WithRotation",
                    testVariationDescription: "LargeVariance",
                    targetLandmarkID: 0,
                    startPoseInMetersAndRads: new Pose2D(16, 60, 0),
                    startPoseVarianceInMetersAndRads: new Pose2D(0.5, 0.5, 0.3),
                    particleCount: 200,
                    translationErrorThreshold: 0.3,
                    headingErrorThreshold: 0.2));
        }

        /// <summary>
        /// Runs a recording from DB config with specified start pose settings.
        /// If a test fails - we generate a 'repro' manifest with settings that made it fail
        /// In this test, 033 was originally facing north (on the map), at the center of the hallway.
        /// Robot started facing north, then turned 270 degrees CW, then 90 degrees CCW, then drove more or less straight to LM0
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Metrics")]
        [TestCategory("Slow")]
        [TestCategory("ShortRecording")]
        public void CenterOfHallwayToLM0WithRotation_WrongPose()
        {
            Assert.IsTrue(
                this.ShortRecordingLocalizationIsWithinThreshold(
                    recordingFolderName: "CenterOfHallwayToLM0WithRotation",
                    testVariationDescription: "WrongPose",
                    targetLandmarkID: 0,
                    startPoseInMetersAndRads: new Pose2D(16 + 0.5, 59.7 + 0.5, 0 + 0.25), // note error introduced to start pose
                    startPoseVarianceInMetersAndRads: new Pose2D(0.0, 0.0, 0.0),
                    particleCount: 200,
                    translationErrorThreshold: 0.3,
                    headingErrorThreshold: 0.2));
        }

        /// <summary>
        /// Runs a recording from DB config with specified start pose settings.
        /// If a test fails - we generate a 'repro' manifest with settings that made it fail
        /// In this test, 033 started in the middle of the corridor near the $Foosball$ area, facing LM0 (on the map).
        /// Placed a new obstacle (cart) in the middle of the corridor and avoided it as an obstacle (stop, turn, resume)
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Metrics")]
        [TestCategory("Slow")]
        [TestCategory("ShortRecording")]
        public void FoosballAreaToLM0WithNewObstacle_LargeStartPoseVariance()
        {
            Assert.IsTrue(
                this.ShortRecordingLocalizationIsWithinThreshold(
                    recordingFolderName: "FoosballAreaToLM0WithNewObstacle",
                    testVariationDescription: "LargeVariance",
                    targetLandmarkID: 0,
                    startPoseInMetersAndRads: new Pose2D(22.5, 59.7, Math.PI),
                    startPoseVarianceInMetersAndRads: new Pose2D(0.5, 0.5, 0.3),
                    particleCount: 200,
                    translationErrorThreshold: 0.3,
                    headingErrorThreshold: 0.2));
        }

        /// <summary>
        /// Runs a recording from DB config with specified start pose settings.
        /// If a test fails - we generate a 'repro' manifest with settings that made it fail
        /// In this test, 033 started in the middle of the corridor near the $Foosball$ area, facing LM0 (on the map).
        /// Placed a new obstacle (cart) in the middle of the corridor and avoided it as an obstacle (stop, turn, resume)
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Metrics")]
        [TestCategory("Slow")]
        [TestCategory("ShortRecording")]
        public void FoosballAreaToLM0WithNewObstacle_WrongPose()
        {
            Assert.IsTrue(
                this.ShortRecordingLocalizationIsWithinThreshold(
                    recordingFolderName: "FoosballAreaToLM0WithNewObstacle",
                    testVariationDescription: "WrongPose",
                    targetLandmarkID: 0,
                    startPoseInMetersAndRads: new Pose2D(22.5 + 0.5, 59.7 + 0.5, Math.PI + 0.25), // note error introduced to start pose
                    startPoseVarianceInMetersAndRads: new Pose2D(0.0, 0.0, 0.0),
                    particleCount: 200,
                    translationErrorThreshold: 0.3,
                    headingErrorThreshold: 0.2));
        }

        /// <summary>
        /// Runs a recording from DB config with specified start pose settings.
        /// If a test fails - we generate a 'repro' manifest with settings that made it fail
        /// In this test, 033 Robot was originally facing west (on the map), at the center of the baker area corridor (facing the exit door)
        /// As it turned and started moving north towards LM1 - a person walked towards it, and as soon as that person passed the robot, another person passed robot from behind
        /// Robot was driven mostly straight with a small stop to avoid the first person.
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Metrics")]
        [TestCategory("Slow")]
        [TestCategory("ShortRecording")]
        public void CenterOfHallwayToLM1WithPeople_LargeStartPoseVariance()
        {            
            Assert.IsTrue(
                this.ShortRecordingLocalizationIsWithinThreshold(
                    recordingFolderName: "CenterOfHallwayToLM1WithPeople",
                    testVariationDescription: "LargeVariance",
                    targetLandmarkID: 1,
                    startPoseInMetersAndRads: new Pose2D(18, 59.7, Math.PI / 2),
                    startPoseVarianceInMetersAndRads: new Pose2D(0.5, 0.5, 0.3),
                    particleCount: 200,
                    translationErrorThreshold: 1, // Note! There appears to be a consistent (and invisible / unexplainable) error in Y of about 65 cm on this recording.
                    headingErrorThreshold: 0.2));
        }

        /// <summary>
        /// Runs a recording from DB config with specified start pose settings.
        /// If a test fails - we generate a 'repro' manifest with settings that made it fail
        /// In this test, 033 Robot was originally facing west (on the map), at the center of the baker area corridor (facing the exit door)
        /// As it turned and started moving north towards LM1 - a person walked towards it, and as soon as that person passed the robot, another person passed robot from behind
        /// Robot was driven mostly straight with a small stop to avoid the first person.
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Metrics")]
        [TestCategory("Slow")]
        [TestCategory("ShortRecording")]
        public void CenterOfHallwayToLM1WithPeople_WrongPose()
        {
            // Note! There appears to be a consistent (and invisible / unexplainable) error in Y of about 65 cm on this recording.
            Assert.IsTrue(
                this.ShortRecordingLocalizationIsWithinThreshold(
                    recordingFolderName: "CenterOfHallwayToLM1WithPeople",
                    testVariationDescription: "WrongPose",
                    targetLandmarkID: 1,
                    startPoseInMetersAndRads: new Pose2D(18 + 0.5, 59.7 + 0.5, Math.PI / 2 + 0.25), // note error introduced to start pose
                    startPoseVarianceInMetersAndRads: new Pose2D(0, 0, 0),
                    particleCount: 200,
                    translationErrorThreshold: 1, // Note! There appears to be a consistent (and invisible / unexplainable) error in Y of about 65 cm on this recording.
                    headingErrorThreshold: 0.2));
        }

        /// <summary>
        /// Optional backdoor method of running UTs w/o touching a database. Useful for quick verification before adding a proper UT (sort of like above)
        /// Runs all sub-folder recordings from a given root folder, and tries different variations of start pose as defined in the settings file
        /// If a given test fails - we generate a 'repro' manifest with settings that made it fail
        /// To test a new recording - drop a folder with recordings and valid PlaybackSettings.xml into the main testing directory. No code 
        /// changes necessary. This is another way to run all tests w/o having to touch DB or code.
        /// Our main method remains to run it from DB configuration, though, as done in above tests. 
        /// Also, if you copy the maps and recording to a local folder and point this test to that folder - tests may run much faster, since those large files
        /// wont have to be pulled down from the server
        /// </summary>
        [Priority(1)]
        [TestMethod] 
        [TestCategory("Metrics")]
        [TestCategory("Slow")]
        [TestCategory("ShortRecording")]
        [Ignore]                            // Disabled for automated runs, useful for quick manual runs, just drop a folder with recordings somewhere 
        public void TestAllShortNavRecordingsFromRootFolder()
        {
            bool overallResult = this.RunShortLocalizationExperimentsOnAllRecordingsFromFolder(
                recordingTopLevelFolder: @"\\robofile\baker\Shared\Users\roboim\Recordings\Shorts\Recordings");

            /*
            bool overallResult = this.RunShortLocalizationExperimentsOnAllRecordingsFromFolder(
                recordingTopLevelFolder: @"\\robofile\baker\Shared\Users\roboim\Recordings\Shorts\Recordings\HallwayUpToConfRoom2167");
            */

            if (!overallResult)
            {
                Assert.Inconclusive("At least one short run failed. Repro manifest file(s) named \"ShortRunFailure_*\" have been generated in the manifests folder");
            }
        }

        /// <summary>
        /// Runs recording given all the playback settings
        /// </summary>
        /// <param name="recordingFolderName">Configuration name in the DB, also used in manifest file name generation</param>
        /// <param name="testVariationDescription">Test description. Used in manifest file name generation</param>
        /// <param name="targetLandmarkID">Landmark ID to which we are driving</param>
        /// <param name="startPoseInMetersAndRads">Start pose</param>
        /// <param name="startPoseVarianceInMetersAndRads">Start pose variance</param>
        /// <param name="particleCount">Particle count</param>
        /// <param name="translationErrorThreshold">Translation error threshold</param>
        /// <param name="headingErrorThreshold">Heading error threshold</param>
        /// <param name="manuallySetFinishPose">Optional externally passed finish pose used when no ground truth markers are installed at the finish</param>
        /// <returns>Whether or not final position was within specified thresholds</returns>
        private bool ShortRecordingLocalizationIsWithinThreshold(
            string recordingFolderName,
            string testVariationDescription,
            int targetLandmarkID,
            Pose2D startPoseInMetersAndRads,
            Pose2D startPoseVarianceInMetersAndRads,
            int particleCount,
            double translationErrorThreshold,
            double headingErrorThreshold,
            Pose2D? manuallySetFinishPose = null)
        {
            Configuration config = MetricsTestRunner.GetConfigurationByName(recordingFolderName, true);

            return this.RunAndVerifyRecording(
                recordingFolderName,
                Path.Combine(config.Recording, recordingFolderName),
                config.Map,
                config.ConfigurationData,
                config.EnvironmentConfiguration,
                testVariationDescription,
                targetLandmarkID,
                startPoseInMetersAndRads,
                startPoseVarianceInMetersAndRads,
                particleCount,
                translationErrorThreshold,
                headingErrorThreshold,
                manuallySetFinishPose);
        }

        /// <summary>
        /// Runs recording given all the playback settings
        /// </summary>
        /// <param name="recordingName">Recording friendly name. Used in manifest file name generation</param>
        /// <param name="recordingFolder">Recording folder</param>
        /// <param name="mapFile">Map folder</param>
        /// <param name="configFolder">Robot configuration folder</param>
        /// <param name="environmentFolder">Environment folder</param>
        /// <param name="testVariationDescription">Test description. Used in manifest file name generation</param>
        /// <param name="targetLandmarkID">Landmark ID to which we are driving</param>
        /// <param name="startPoseInMetersAndRads">Start pose</param>
        /// <param name="startPoseVarianceInMetersAndRads">Start pose variance</param>
        /// <param name="particleCount">Particle count</param>
        /// <param name="translationErrorThreshold">Translation error threshold</param>
        /// <param name="headingErrorThreshold">Heading error threshold</param>
        /// <param name="manuallySetFinishPose">Optional externally passed finish pose used when no ground truth markers are installed at the finish</param>
        /// <returns>Whether or not final position was within specified thresholds</returns>
        private bool RunAndVerifyRecording(
            string recordingName,
            string recordingFolder,
            string mapFile,
            string configFolder,
            string environmentFolder,
            string testVariationDescription,
            int targetLandmarkID,
            Pose2D startPoseInMetersAndRads,
            Pose2D startPoseVarianceInMetersAndRads,
            int particleCount,
            double translationErrorThreshold,
            double headingErrorThreshold,
            Pose2D? manuallySetFinishPose)
        {
            var manifestAndAgentTuple = this.RunShortRecordingManifest(
                    recordingFolder,
                    mapFile,
                    configFolder,
                    environmentFolder,
                    targetLandmarkID,
                    startPoseInMetersAndRads,
                    startPoseVarianceInMetersAndRads,
                    particleCount,
                    manuallySetFinishPose);

            Manifest manifest = manifestAndAgentTuple.Item1;
            ShortNavRunVerifierAgent verifierTestAgent = manifestAndAgentTuple.Item2;

            if (Math.Abs(verifierTestAgent.DeltaError.Heading) > headingErrorThreshold ||
                Math.Abs(verifierTestAgent.DeltaError.X) > translationErrorThreshold ||
                Math.Abs(verifierTestAgent.DeltaError.Y) > translationErrorThreshold)
            {
                // In case of failure - save the manifest in XML format so that we can run repro in normal mode with UI and all that jazz (use /o option)
                string manifestFileName = string.Format(@"C:\temp\FailedShortRunManifests\ShortRunFailure_{0}_{1}.xml", recordingName, testVariationDescription);

                try
                {
                    Directory.CreateDirectory(@"C:\temp\FailedShortRunManifests");
                    Serializer.Save(manifestFileName, manifest, manifest.GetKnownTypes());
                }
                catch (Exception e)
                {
                    // We do not care if saving file on a build server fails, and there is nothing we can about it anyway.
                    // Might as well let the test fail gracefully (as it is about to) and just log the fact we cant write the manifest
                    Console.WriteLine("Cannot save the manifest. Suspect temporary folder on the build machine. " + e.ToString());
                }

                return false;
            }

            return true;
        }

        /// <summary>
        /// Run the recording once given all run parameters. Will block until agent returns
        /// </summary>
        /// <param name="recordingsDirectory">The directory that contains the recorded files</param>
        /// <param name="mapFile">The map to load and localize on</param>
        /// <param name="robotConfigFolder">Calibration Data Folder</param>
        /// <param name="environmentConfigFolder">Environment config folder, where LandmarksInfo.xml for a map we use lives</param>
        /// <param name="targetLandmarkID">Landmark ID to which we are driving</param>
        /// <param name="startPoseInMetersAndRads">The start pose of the robot, relative to the bottom right corner of the map, in map units.</param>
        /// <param name="startPoseVarianceInMetersAndRads">The uncertainty in the initial pose estimate</param>
        /// <param name="particleCount">Count of particles used for localization.</param>
        /// <param name="manuallySetFinishPose">Optional externally passed finish pose used when no ground truth markers are installed at the finish</param>
        /// <returns>Tuple of SLAM metrics manifest and the agent that we can query to get the numeric results</returns>
        private Tuple<Manifest, ShortNavRunVerifierAgent> RunShortRecordingManifest(
            string recordingsDirectory,
            string mapFile,
            string robotConfigFolder,
            string environmentConfigFolder,
            int targetLandmarkID,
            Pose2D startPoseInMetersAndRads,
            Pose2D startPoseVarianceInMetersAndRads,
            int particleCount,
            Pose2D? manuallySetFinishPose)
        {
            // manifest that runs the localization metrics calculation against a set of recordings
            var manifestAndAgentTuple = NavigationManifest.CreateLocalizationMiniRunTestManifest(
                recordingsDirectory,
                mapFile,
                targetLandmarkID,
                startPoseInMetersAndRads,
                startPoseVarianceInMetersAndRads,
                robotConfigFolder,
                environmentConfigFolder,
                particleCount,
                manuallySetFinishPose);

            Manifest manifest = manifestAndAgentTuple.Item1;
            ShortNavRunVerifierAgent verifierTestAgent = manifestAndAgentTuple.Item2;

            // Add collector so that we know when its time to process results
            MessageCollectorAgent<CompletionMessage> collector = new MessageCollectorAgent<CompletionMessage>("collector", verifierTestAgent);
            manifest.Agents.Add(collector);
            manifest.ActiveAgents.Add(collector.Name);

            AgentLocator locator = new AgentLocator(true);
            locator.LoadFrom(manifest);

            // Wait until collector returns
            while (collector.Results.Count <= 0)
            {
                collector.Wait(1000);
            }

            // Giving some additional time if test machine is slow will ensure we get to the last messages in the queues
            collector.Wait(3000);

            locator.Deactivate();

            // we dont want the collector in the manifest we might be saving to the disk
            manifest.Agents.Remove(collector);
            manifest.ActiveAgents.Remove(collector.Name);

            return new Tuple<Manifest, ShortNavRunVerifierAgent>(manifest, verifierTestAgent);
        }              

        /*
         Below code is an alternative way of running variations faster w/o having to go to the database. It uses PlaybackSettings.xml from local
         recording folder.
         This is completely for faster debugging and variation testing purpose, if you prefer database - driven testin (above UTs), use those, ignore
         code below.
        */        

        /// <summary>
        /// Top level call that traverses the folder, and runs all recordings from all sub-folders or top level folder if there are recordings there
        /// </summary>
        /// <param name="recordingTopLevelFolder">The directory that contains the recorded files</param>
        /// <returns>Whether or not all variations from all recording sub-folders succeeded</returns>
        private bool RunShortLocalizationExperimentsOnAllRecordingsFromFolder(
            string recordingTopLevelFolder)
        {
            bool overallResult = true;

            // Settings file exists at top level - just run that
            if (File.Exists(Path.Combine(recordingTopLevelFolder, "PlaybackSettings.xml")))
            {
                string recordingFriendlyName = this.GetRecordingFriendlyNameFromFolderName(recordingTopLevelFolder);

                overallResult = this.RunRecordingFromFolder(recordingTopLevelFolder, recordingFriendlyName);
            }
            else
            {
                var folderList = Directory.EnumerateDirectories(recordingTopLevelFolder);

                foreach (var folderName in folderList)
                {
                    string recordingFriendlyName = this.GetRecordingFriendlyNameFromFolderName(recordingTopLevelFolder);

                    bool result = this.RunRecordingFromFolder(folderName, recordingFriendlyName);

                    overallResult = !result ? result : overallResult;
                }
            }

            return overallResult;
        }

        /// <summary>
        /// Given a folder path - extracts the "recording friendly name" which is nothing more than the last folder name in the path
        /// </summary>
        /// <param name="recordingTopLevelFolder">Recording folder path</param>
        /// <returns>Last folder name in the path to be used as a 'recording friendly name'</returns>
        private string GetRecordingFriendlyNameFromFolderName(string recordingTopLevelFolder)
        {
            string[] subpaths = recordingTopLevelFolder.Split('\\');
            string recordingFriendlyName = subpaths[subpaths.Length - 1];
            return recordingFriendlyName;
        }

        /// <summary>
        /// Runs recording from a given folder
        /// </summary>
        /// <param name="folderName">Folder name to run recording from</param>
        /// <param name="recordingFriendlyName">User-friendly name for the test variation</param>
        /// <returns>Whether or not all variations succeeded</returns>
        private bool RunRecordingFromFolder(string folderName, string recordingFriendlyName)
        {
            string mapFile = null, robotConfigFolder = null, environmentConfigFolder = null;
            int targetLandmarkID = 0;
            List<ExperimentVariation> variations = null;

            Pose2D? finishPose = null;

            // For testing purposes and during test case creation, a recording can have a settings file, 
            // which defines properties of the recording, such as the start pose, as well as
            // other settings necessary to test the recording in the right environment and with desired variations.                
            this.GetRecordingPlaybackSettings(
                folderName,
                out mapFile,
                out robotConfigFolder,
                out environmentConfigFolder,
                out targetLandmarkID,
                out variations,
                out finishPose);

            // run all variations of this recording
            return this.RunRecordingWithStartPoseVariations(
                folderName,
                mapFile,
                targetLandmarkID,
                robotConfigFolder,
                environmentConfigFolder,
                recordingFriendlyName,
                variations,
                finishPose);
        }

        /// <summary>
        /// Runs a single recording with start pose variations
        /// </summary>
        /// <param name="recordingFolder">The directory that contains the recorded files</param>
        /// <param name="mapFile">The map to load and localize on</param>
        /// <param name="targetLandmarkID">Landmark ID to which we are driving</param>
        /// <param name="robotConfigFolder">Calibration Data Folder</param>
        /// <param name="environmentConfigFolder">Environment config folder, where LandmarksInfo.xml for a map we use lives</param>
        /// <param name="recordingName">Count of particles used for localization.</param>
        /// <param name="variations">List of start pose and experiment variations to try on this recording</param>
        /// <param name="manuallySetFinishPose">Optional externally passed finish pose used when no ground truth markers are installed at the finish</param>
        /// <returns>Whether or not all variations of this experiment succeeded</returns>
        private bool RunRecordingWithStartPoseVariations(
            string recordingFolder,
            string mapFile,
            int targetLandmarkID,
            string robotConfigFolder,
            string environmentConfigFolder,
            string recordingName,
            List<ExperimentVariation> variations,
            Pose2D? manuallySetFinishPose)
        {
            bool allSucceeded = true;

            foreach (var experimentVariation in variations)
            {
                bool succeeded = this.RunAndVerifyRecording(
                    recordingName,
                    recordingFolder,
                    mapFile,
                    robotConfigFolder,
                    environmentConfigFolder,
                    experimentVariation.FriendlyName,
                    targetLandmarkID,
                    experimentVariation.StartPose,
                    experimentVariation.StartPoseVarianceInMetersAndRads,
                    experimentVariation.ParticleCount,
                    experimentVariation.TranslationErrorThreshold,
                    experimentVariation.HeadingErrorThreshold,
                    manuallySetFinishPose);

                if (!succeeded)
                {
                    allSucceeded = false;
                }
            }

            return allSucceeded;
        }  

        /// <summary>
        /// Reads test settings for this recording from xml. It allows us add more navigation tests by merely 
        /// dropping a new folder into the main recordings folder as opposed to modifying the code
        /// </summary>
        /// <param name="recordingFolder">Recording folder</param>
        /// <param name="mapFile">Map file name</param>
        /// <param name="robotConfigFolder">Robot config folder name</param>
        /// <param name="environmentConfigFolder">Environment config folder name</param>
        /// <param name="targetLandmarkID">Landmark ID to which we are driving</param>
        /// <param name="variations">List of start pose variations to run on this recording</param>
        /// <param name="manuallySetFinishPose">Optional externally passed finish pose used when no ground truth markers are installed at the finish</param>
        private void GetRecordingPlaybackSettings(
            string recordingFolder,
            out string mapFile,
            out string robotConfigFolder,
            out string environmentConfigFolder,
            out int targetLandmarkID,
            out List<ExperimentVariation> variations,
            out Pose2D? manuallySetFinishPose)
        {
            /*
              Recommended: Add a PlaybackSettings.xml - by following examples from other recordings. Its a simple file that allows 0 code changes 
              to run new tests, and avoids having to mod the database or duplicate the UT code for each variation. 
              See TestAllShortNavRecordingsFromRootFolder test, which runs all recordings from the root folder of short recordings
             */
            /*
            example format of the setting file
            <RecordingPlaybackSettings>
                <MapFile>\\sbgfile\Baker\Shared\Users\roboim\Recordings\Shorts\Maps\BigCementMap.xml</MapFile>
                <TargetLandmarkID>0</TargetLandmarkID>
                <RobotConfigFolder>\\sbgfile\baker\Shared\Users\roboim\RobotConfig\EV4A-033</RobotConfigFolder>
                <EnvironmentConfigFolder>\\sbgfile\Baker\Shared\Users\roboim\Recordings\Shorts\Maps</EnvironmentConfigFolder>
                <StartPoseX>18</StartPoseX>
                <StartPoseY>30</StartPoseY>
                <StartPoseHeadingDegrees>90</StartPoseHeadingDegrees>
                <StartPoseVariation FriendlyName="LargeVarianceInTranslationAndOrientation" X="100" Y="100" HeadingInDegrees="60" ParticleCount="5" FailIfTranslationErrorInMetersExcceeds="0.3" FailIfHeadingErrorInRadiansExcceeds="0.25"/>
                <StartPoseVariation FriendlyName="ModerateVarianceInTranslationAndOrientation" X="50" Y="50" HeadingInDegrees="30" ParticleCount="5" FailIfTranslationErrorInMetersExcceeds="0.3" FailIfHeadingErrorInRadiansExcceeds="0.25" AddStartPoseXError="10" AddStartPoseYError="20" AddStartPoseHeadingErrorInDegrees="30"/>
                <StartPoseVariation ... any number of those/>
            </RecordingPlaybackSettings>
              
            Finish pose is optional, but can also be defined
            <RecordingPlaybackSettings>
                <MapFile>\\sbgfile\Baker\Shared\Users\roboim\Recordings\Shorts\Maps\BigCementMap.xml</MapFile>
                <TargetLandmarkID>0</TargetLandmarkID>
                <RobotConfigFolder>\\sbgfile\baker\Shared\Users\roboim\RobotConfig\EV4A-033</RobotConfigFolder>
                <EnvironmentConfigFolder>\\sbgfile\Baker\Shared\Users\roboim\Recordings\Shorts\Maps</EnvironmentConfigFolder>
                <StartPoseX>18</StartPoseX>
                <StartPoseY>30</StartPoseY>
                <StartPoseHeadingDegrees>90</StartPoseHeadingDegrees>             
                <FinishPoseX>20</FinishPoseX>
                <FinishPoseX>25</FinishPoseX>
                <FinishPoseHeadingInDegrees>-90</FinishPoseHeadingInDegrees>  
                <StartPoseVariation FriendlyName="LargeVarianceInTranslationAndOrientation" X="100" Y="100" HeadingInDegrees="60" ParticleCount="5" FailIfTranslationErrorInMetersExcceeds="0.3" FailIfHeadingErrorInRadiansExcceeds="0.25"/>
                <StartPoseVariation FriendlyName="ModerateVarianceInTranslationAndOrientation" X="50" Y="50" HeadingInDegrees="30" ParticleCount="5" FailIfTranslationErrorInMetersExcceeds="0.3" FailIfHeadingErrorInRadiansExcceeds="0.25" AddStartPoseXError="10" AddStartPoseYError="20" AddStartPoseHeadingErrorInDegrees="30"/>
                <StartPoseVariation ... any number of those/>
            </RecordingPlaybackSettings>
             * 
            */

            manuallySetFinishPose = null;

            XmlDocument xmlDoc = new XmlDocument();
            xmlDoc.Load(recordingFolder + "\\PlaybackSettings.xml");

            // Get elements
            mapFile = xmlDoc.GetElementsByTagName("MapFile")[0].InnerText;
            targetLandmarkID = int.Parse(xmlDoc.GetElementsByTagName("TargetLandmarkID")[0].InnerText);
            robotConfigFolder = xmlDoc.GetElementsByTagName("RobotConfigFolder")[0].InnerText;
            environmentConfigFolder = xmlDoc.GetElementsByTagName("EnvironmentConfigFolder")[0].InnerText;            

            variations = new List<ExperimentVariation>();

            int variationElementIndex = 0;
            var variationNode = xmlDoc.GetElementsByTagName("StartPoseVariation")[variationElementIndex];            

            while (null != variationNode) 
            {
                ExperimentVariation variation = new ExperimentVariation();

                variation.StartPoseVarianceInMetersAndRads = new Pose2D(
                    double.Parse(variationNode.Attributes["VarianceXInMeters"].InnerText),
                    double.Parse(variationNode.Attributes["VarianceYInMeters"].InnerText),
                    double.Parse(variationNode.Attributes["VarianceHeadingInDegrees"].InnerText) * MathConstants.Degrees2Radians);

                variation.ParticleCount = int.Parse(variationNode.Attributes["ParticleCount"].InnerText);
                variation.TranslationErrorThreshold = double.Parse(variationNode.Attributes["FailIfTranslationErrorInMetersExcceeds"].InnerText);
                variation.HeadingErrorThreshold = double.Parse(variationNode.Attributes["FailIfHeadingErrorInRadiansExcceeds"].InnerText);
                variation.FriendlyName = variationNode.Attributes["FriendlyName"].InnerText;

                // To allow simulated start pose errors (which are different from variance) we recognize below attrubutes and add them to the start
                // pose for each experiment variation
                double startPoseXAdditionalError = 0;
                if (null != variationNode.Attributes["AddStartPoseXError"])
                {
                    startPoseXAdditionalError = double.Parse(variationNode.Attributes["AddStartPoseXError"].InnerText);
                }

                double startPoseYAdditionalError = 0;
                if (null != variationNode.Attributes["AddStartPoseYError"])
                {
                    startPoseYAdditionalError = double.Parse(variationNode.Attributes["AddStartPoseYError"].InnerText);
                }

                double startPoseHeadingAdditionalError = 0;
                if (null != variationNode.Attributes["AddStartPoseHeadingErrorInDegrees"])
                {
                    startPoseHeadingAdditionalError = double.Parse(variationNode.Attributes["AddStartPoseHeadingErrorInDegrees"].InnerText);
                }

                double startXinMeters     = double.Parse(xmlDoc.GetElementsByTagName("StartPoseX")[0].InnerText) + startPoseXAdditionalError;
                double startYInMeters     = double.Parse(xmlDoc.GetElementsByTagName("StartPoseY")[0].InnerText) + startPoseYAdditionalError;
                double startHeadingInRads = double.Parse(xmlDoc.GetElementsByTagName("StartPoseHeadingDegrees")[0].InnerText) * MathConstants.Degrees2Radians + startPoseHeadingAdditionalError;

                if (null != xmlDoc.GetElementsByTagName("FinishPoseX")[0])
                {
                    // We assume that if FinishPoseX does not exist - finish pose is not specified, and if FinishPoseX is specified - we expect
                    // all other finihs pose fields to be initialized too (or else we crash)
                    double finishXinMeters = double.Parse(xmlDoc.GetElementsByTagName("FinishPoseX")[0].InnerText) + startPoseXAdditionalError;
                    double finishYInMeters = double.Parse(xmlDoc.GetElementsByTagName("FinishPoseY")[0].InnerText) + startPoseYAdditionalError;
                    double finishHeadingInRads = double.Parse(xmlDoc.GetElementsByTagName("FinishPoseHeadingDegrees")[0].InnerText) * MathConstants.Degrees2Radians + startPoseHeadingAdditionalError;

                    manuallySetFinishPose = new Pose2D(finishXinMeters, finishYInMeters, finishHeadingInRads);
                }

                variation.StartPose = new Pose2D(startXinMeters, startYInMeters, startHeadingInRads);

                variations.Add(variation);

                variationNode = xmlDoc.GetElementsByTagName("StartPoseVariation")[++variationElementIndex];
            }
        }

        /// <summary>
        /// Helper class that holds variations of start pose and a few other settings that will be used with the recording
        /// This way we can run multiple experiments with the the same recording, i.e. specify higher or lower variances, change particle 
        /// count, or intentionally introduce start pose errors
        /// </summary>
        internal class ExperimentVariation 
        {
            /// <summary>
            /// Gets or sets start pose for an experiment
            /// </summary>
            public Pose2D StartPose { get; set; }

            /// <summary>
            /// Gets or sets start pose variation (in meters and radians) for an experiment
            /// </summary>
            public Pose2D StartPoseVarianceInMetersAndRads { get; set; }

            /// <summary>
            /// Gets or sets particle count
            /// </summary>
            public int ParticleCount { get; set; }

            /// <summary>
            /// Gets or sets the final pose translation error beyond which we will consider variation test as a failure
            /// </summary>
            public double TranslationErrorThreshold { get; set; }

            /// <summary>
            /// Gets or sets the final pose heading error beyond which we will consider variation test as a failure
            /// </summary>
            public double HeadingErrorThreshold { get; set; }

            /// <summary>
            /// Gets or sets variation friendly name to be used in repro manifest file name if test fails
            /// </summary>
            public string FriendlyName { get; set; }
        }
    }
}
